
package common;

import common.packet.BodyState;
import common.packet.ClientInfo;
import java.util.HashMap;
import java.util.Iterator;
import java.util.Map;
import java.util.concurrent.ConcurrentHashMap;
import org.jbox2d.collision.AABB;
import org.jbox2d.collision.CircleDef;
import org.jbox2d.collision.PolygonDef;
import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.BodyDef;
import org.jbox2d.dynamics.ContactListener;
import org.jbox2d.dynamics.World;
import org.jbox2d.dynamics.contacts.ContactPoint;
import org.jbox2d.dynamics.contacts.ContactResult;

/**
 * Wrapper for the Dynamic Button Soccer's physics world.
 * 
 * @author xissburg
 */
public class PhysicsWorld
{
    private final World world;

    private Body middleLeftBody;
    private Body middleRightBody;
    private Body middleCircleBody;

    private Body leftGoalSensor;
    private Body rightGoalSensor;

    private Map<String, Body> bodyMap;

    /**
     * Maps pairs of bodies to contact listeners. When a contact event happens,
     * the colliding pair is identified and if there's an entry for this pair
     * in this map, the event handling is delegated to the corresponding
     * ContactListener
     */
    private Map<BodyPair, ContactListener> collisionPairs;

    /**
     * Stores forces to be applied on bodies for an amount of time.
     * The applyTimedForce method adds an entry to this map and before each
     * world.step() this map is iterated and for each entry, if the time has not
     * yet expired it applies the force on the body, otherwise it removes the
     * entry from this map.
     */
    private Map<Body, TimedForce> timedForceMap;

    //private static final float timeStep = 1.f/60.f;
    private static final int numIterations = 10;
    private long time;

    public static final float newtonsPerPixel = 0.5f;
    public static final float maxForce = 20.f;

    public PhysicsWorld()
    {
        //initialize physics engine and setup static objects(walls) and ball
        world = new World(new AABB(new Vec2(-200.f,-200.f), new Vec2(200.f, 200.f)),
                new Vec2(0.f, 0.f), true);
        bodyMap = new ConcurrentHashMap<String, Body>();
        collisionPairs = new HashMap<BodyPair, ContactListener>();
        timedForceMap = new ConcurrentHashMap<Body, TimedForce>();
        initializeObjects();
    }

    private void initializeObjects()
    {
        synchronized(world) {
            //add static bodies, walls...
            //top wall
            BodyDef def = new BodyDef();
            def.position.set(0.f, 6.2f);
            Body body = world.createBody(def);
            PolygonDef pDef = new PolygonDef();
            pDef.setAsBox(9.2f, 1.f);
            pDef.friction = 0.3f;
            pDef.restitution = 0.3f;
            body.createShape(pDef);

            //bottom wall
            def.position.set(0.f, -6.2f);
            body = world.createBody(def);
            body.createShape(pDef);

            //left wall
            def.position.set(-10.f, 0.f);
            body = world.createBody(def);
            pDef.setAsBox(1.f, 7.2f);
            pDef.restitution = 0.f;//rear of goal
            body.createShape(pDef);

            def.position.set(-8.7f, 3.1f);
            body = world.createBody(def);
            pDef.setAsBox(0.3f, 2.1f);
            pDef.restitution = 0.3f;
            body.createShape(pDef);

            def.position.set(-8.7f, -3.1f);
            body = world.createBody(def);
            body.createShape(pDef);

            //right wall
            def.position.set(10.f, 0.f);
            body = world.createBody(def);
            pDef.setAsBox(1.f, 7.2f);
            pDef.restitution = 0.f;
            body.createShape(pDef);

            def.position.set(8.7f, 3.1f);
            body = world.createBody(def);
            pDef.setAsBox(0.3f, 2.1f);
            pDef.restitution = 0.3f;
            body.createShape(pDef);

            def.position.set(8.7f, -3.1f);
            body = world.createBody(def);
            body.createShape(pDef);

            //left goal sensor
            def.position.set(-8.8f, 0.f);
            leftGoalSensor = world.createBody(def);
            pDef.setAsBox(0.2f, 1.f);
            pDef.isSensor = true;
            leftGoalSensor.createShape(pDef);

            //right goal sensor
            def.position.set(8.8f, 0.f);
            rightGoalSensor = world.createBody(def);
            rightGoalSensor.createShape(pDef);

            //create dynamic bodies
            //create 'ball' and add it to world
            BodyDef ballDef = new BodyDef();
            ballDef.position.set(0.f, 0.f);
            ballDef.linearDamping = 0.4f;
            ballDef.allowSleep = true;
            ballDef.isBullet = true;
            Body ballBody = world.createBody(ballDef);//add to world
            CircleDef cd = new CircleDef();
            cd.radius = 0.1f;
            cd.density = 100.f;
            cd.restitution = 0.8f;
            cd.friction = 0.3f;
            cd.filter.categoryBits = 1;
            cd.filter.maskBits = 1;
            ballBody.createShape(cd);
            ballBody.setMassFromShapes();
            String ballName = "$ball";
            ballBody.setUserData(ballName);
            bodyMap.put(ballName, ballBody);
            
            time = 0;

            world.setContactListener(new _ContactListener());
        }
    }

    public Body getBody(String name) {
        return bodyMap.get(name);
    }

    public BodyState getBodyState(String name)
    {
        Body b = getBody(name);

        if(b == null)
            return null;

        BodyState bs = new BodyState(name);
        Vec2 pos = b.getWorldCenter();
        bs.posX = pos.x; bs.posY = pos.y;
        bs.angle = b.getAngle();
        Vec2 vel = b.getLinearVelocity();
        bs.linVelX = vel.x; bs.linVelY = vel.y;
        bs.angVel = b.getAngularVelocity();
        bs.forceX = b.m_force.x; bs.forceY = b.m_force.y;
        bs.torque = b.m_torque;

        return bs;
    }

    public void setBodyState(BodyState bs)
    {
        Body b = getBody(bs.name);

        if(b == null)
            return;

        Vec2 pos = new Vec2(bs.posX,bs.posY);
        b.setXForm(pos, bs.angle);
        b.setLinearVelocity(new Vec2(bs.linVelX,bs.linVelY));
        b.setAngularVelocity(bs.angVel);
        //b.applyForce(new Vec2(bs.forceX, bs.forceY), pos);
        b.applyTorque(bs.torque);

        if(bs.forceX != 0f || bs.forceY != 0f)
            applyTimedForceOnBody(bs.name, new Vec2(bs.forceX, bs.forceY), 50);
    }

    public void resetBall()
    {
        Body ball = bodyMap.get("$ball");
        ball.setXForm(new Vec2(0,0), ball.getAngle());
        ball.setLinearVelocity(new Vec2(0,0));
        ball.setAngularVelocity(0);
    }

    /**
     * Adds the middle separators to the world. These stops the buttons from
     * invading the others team's field.
     */
    public void addMiddleSeparators()
    {
        //synchronize in world so that this won't run during a step or something.
        //Same for the other methods below.
        synchronized(world) {
            //create and add static bodies
            BodyDef def = new BodyDef();
            def.position.set(-0.5f, 0.f);
            middleLeftBody = world.createBody(def);
            PolygonDef pDef = new PolygonDef();
            pDef.setAsBox(0.5f, 7.5f);
            pDef.filter.categoryBits = 2;
            pDef.filter.maskBits = 1;
            middleLeftBody.createShape(pDef);

            def.position.set(0.5f, 0.f);
            middleRightBody = world.createBody(def);
            pDef.filter.categoryBits = 4;
            pDef.filter.maskBits = 1;
            middleRightBody.createShape(pDef);

            def.position.set(0.f, 0.f);
            middleCircleBody = world.createBody(def);
            CircleDef cDef = new CircleDef();
            cDef.radius = 1.6f;
            cDef.filter.categoryBits = 8;
            cDef.filter.maskBits = 1;
            middleCircleBody.createShape(cDef);
        }
    }

    public void removeMiddleSeparators()
    {
        synchronized(world) {
            world.destroyBody(middleLeftBody);
            world.destroyBody(middleRightBody);
            world.destroyBody(middleCircleBody);
        }
    }

    /**
     * Adds a button(circle) to the world and uses the string name as
     * its ID.
     *
     * @param name The name/ID of the button
     * @param team The team the button belongs to.
     */
    public void addButton(String name, float posX, float posY, ClientInfo.Team team)
    {
        synchronized(world) {
            //if there's a button with the same name in bodyMap first remove it.
            //There cannot be buttons with the same name.
            if(bodyMap.get(name) != null)
                removeBody(name);

            BodyDef buttonBodyDef = new BodyDef();
            buttonBodyDef.linearDamping = 0.5f;
            buttonBodyDef.allowSleep = true;

            CircleDef buttonShapeDef = new CircleDef();
            buttonShapeDef.radius = 0.3f;
            buttonShapeDef.density = 2.f;
            buttonShapeDef.friction = 0.4f;
            buttonShapeDef.restitution = 0.7f;
            buttonShapeDef.filter.categoryBits = 1;
            buttonBodyDef.position.set(posX, posY);
            
            Body b = world.createBody(buttonBodyDef);

            //Set filter mask bits for proper filtering depending on team
            if(team == ClientInfo.Team.RIGHT)
                buttonShapeDef.filter.maskBits = 1 | 2 | 8;
            else if(team == ClientInfo.Team.LEFT)
                buttonShapeDef.filter.maskBits = 1 | 4 | 8;

            b.createShape(buttonShapeDef);
            b.setMassFromShapes();
            //store the button name in the user data so that it is possible to
            //retrieve its key in the bodyMap
            b.setUserData(name);
            bodyMap.put(name, b);
        }
    }

    public void setButtonState(String name, float posX, float posY, ClientInfo.Team team)
    {
        synchronized(world) {
            Body b = bodyMap.get(name);

            if(b != null)
            {
                setBodyPosition(name, posX, posY);
                
                if(team == ClientInfo.Team.RIGHT)
                    b.getShapeList().getFilterData().maskBits = 1 | 2 | 8;
                else if(team == ClientInfo.Team.LEFT)
                    b.getShapeList().getFilterData().maskBits = 1 | 4 | 8;
            }
        }
    }

    public void removeBody(String name)
    {
        synchronized(world) {
            Body b = bodyMap.get(name);

            if(b != null)
            {
                world.destroyBody(b);
                bodyMap.remove(name);
            }
        }
    }

    public void setBodyPosition(String name, float x, float y)
    {
        synchronized(world) {
            Body b = bodyMap.get(name);

            if(b != null)
            {
                b.setXForm(new Vec2(x,y), b.getAngle());
            }
        }
    }

    public void setBodyVelocity(String name, float x, float y)
    {
        synchronized(world) {
            Body b = bodyMap.get(name);
            b.setLinearVelocity(new Vec2(x,y));
            b.wakeUp();//tricky
        }
    }

    public void applyForceOnBody(String name, Vec2 f)
    {
        synchronized(world) {
            Body b = bodyMap.get(name);
            b.applyForce(f, b.getWorldCenter());
        }
    }

    public void applyForceOnBody(String name, float fx, float fy)
    {
        applyForceOnBody(name, new Vec2(fx,fy));
    }

    public Vec2 computeForceFromDisplacement(int dx, int dy)
    {
        Vec2 force = new Vec2(dx*newtonsPerPixel, dy*newtonsPerPixel);

        //clamp to maxForce
        if(force.lengthSquared() > maxForce*maxForce)
        {
            force.normalize();
            force.mulLocal(maxForce);
        }

        return force;
    }

    /**
     * Applies a force on a button continuously for an amount of time. Basically,
     * the force specified will be applied before stepping the simulation until
     * the time expires.
     *
     * @param name Button name/ID
     * @param f The force to be applied
     * @param duration The duration in milliseconds
     */
    public void applyTimedForceOnBody(String name, Vec2 f, long duration)
    {
        Body b = bodyMap.get(name);

        if(b == null)
            return;
        
        //Specifify duration < 0 so that the time stamp will be set in applyTimedForces.
        //This guarantees that the force will be applied at least once.
        timedForceMap.put(b, new TimedForce(f, -1, duration*(long)1e6));
    }

    /**
     * Apply the timed forces stored in timedForcesMap. For each entry, if the
     * timeStamp is smaller than zero, it applies the force and sets the timeStamp
     * to be t, else if it is in range it applies the force else it removes the
     * entry from the map.
     * @param t
     */
    private void applyTimedForces(long t)
    {
        Iterator<Map.Entry<Body, TimedForce>> it = timedForceMap.entrySet().iterator();

        while(it.hasNext())
        {
            Map.Entry<Body, TimedForce> entry = it.next();

            TimedForce tf = entry.getValue();

            if(tf.timeStamp < 0)
            {
                tf.timeStamp = t;
                Body b = entry.getKey();
                b.applyForce(tf.force, b.getWorldCenter());
            }
            else if(t - tf.timeStamp < tf.duration)
            {
                Body b = entry.getKey();
                b.applyForce(tf.force, b.getWorldCenter());
            }
            else
            {
                it.remove();
            }
        }
    }

    public void start(){
        time = System.nanoTime();
    }

    public void step()
    {
        long t = System.nanoTime();
        long dt = t - time;
        time = t;

        synchronized(world) {
            applyTimedForces(time);
            world.step((float)(dt*1e-9), numIterations);
        }
    }

    public void step(float dt)
    {
        //time = System.nanoTime();
        synchronized(world) {
            applyTimedForces(time);
            world.step(dt, numIterations);
        }
    }

    //ContactListener stuff
    private class _ContactListener implements ContactListener
    {
        public void add(ContactPoint point)
        {
            BodyPair pair = new BodyPair(point.shape1.getBody(), point.shape2.getBody());
            ContactListener cl = collisionPairs.get(pair);

            if(cl != null)
                cl.add(point);
        }

        public void persist(ContactPoint point) 
        {
            BodyPair pair = new BodyPair(point.shape1.getBody(), point.shape2.getBody());
            ContactListener cl = collisionPairs.get(pair);

            if(cl != null)
                cl.persist(point);
        }

        public void remove(ContactPoint point)
        {
            BodyPair pair = new BodyPair(point.shape1.getBody(), point.shape2.getBody());
            ContactListener cl = collisionPairs.get(pair);

            if(cl != null)
                cl.remove(point);
        }

        public void result(ContactResult point) 
        {
            BodyPair pair = new BodyPair(point.shape1.getBody(), point.shape2.getBody());
            ContactListener cl = collisionPairs.get(pair);

            if(cl != null)
                cl.result(point);
        }

    }

    private static class BodyPair
    {
        public final Body bodyA;
        public final Body bodyB;

        public BodyPair(Body bodyA, Body bodyB) {
            this.bodyA = bodyA;
            this.bodyB = bodyB;
        }

        //A pair of bodies is equal if their bodies are the same instances, the
        //order doesn't matter
        @Override
        public boolean equals(Object obj) {
            if (this == obj) {
                return true;
            }
            if (!(obj instanceof BodyPair)) {
                return false;
            }
            final BodyPair other = (BodyPair) obj;
            return ((this.bodyA == other.bodyA && this.bodyB == other.bodyB) ||
                    (this.bodyA == other.bodyB && this.bodyB == other.bodyA));
        }

        @Override
        public int hashCode() {
            int hash = 7;
            hash = 23 * hash + bodyA.hashCode() + bodyB.hashCode();
            return hash;
        }
    }

    private static class TimedForce
    {
        public Vec2 force;
        public long timeStamp;
        public long duration; //duration in nanoseconds

        public TimedForce(Vec2 force, long timeStamp, long duration) {
            this.force = force;
            this.timeStamp = timeStamp;
            this.duration = duration;
        }
    }
}

